#ifndef EI_KALMANFILTERS_H
#define EI_KALMANFILTERS_H

//
// Bayes++ the Bayesian Filtering Library
// Copyright (c) 2002 Michael Stevens
// See accompanying Bayes++.htm for terms and conditions of use.
//

#include <BayesFilter/UDFlt.hpp>
#include <BayesFilter/unsFlt.hpp>
#include <BayesFilter/itrFlt.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <Ogre.h>

using namespace Bayesian_filter;
using namespace Bayesian_filter_matrix;
using namespace Ogre;

// Square 
template <class scalar>
inline scalar sqr(scalar x)
{
	return x*x;
}

//
// Implementation of Bayes++  
// Position and Velocity Prediction model
//
class PVpredict : public Linear_predict_model
{
public:
	PVpredict();
};

//
// Position and Velocity Observation model
//
class PVobserve : public Linrz_uncorrelated_observe_model
{
	mutable Vec z_pred;
public:
	PVobserve ();
	const Vec& h(const Vec& x) const
	{
		z_pred[0] = x[0];
		return z_pred;
	};
};


//
// Basic noise reduction KalmanFilter
//
class BasicPredict : public Linear_predict_model
{
public:
	BasicPredict();

};


class BasicObserve : public Linear_uncorrelated_observe_model
{
public:
	BasicObserve () ;
};

//
// Classes for using Kalman filters in Ei4
//
// Velocity interpolation filter
//
namespace Ei
{
enum {X = 0,Y,Z};

class VelocityInterpolator // will predict a new position according to old position and velocity
{
public:
	VelocityInterpolator();
	~VelocityInterpolator();
	
	void init();
	
	Real Predict();	
	void Measure(Real data);
	
private:
	
	void update();
	// Prediction and Observe model for filtering spikes.
	
	// Prediction and Observe model for Velocity, To estimate position.
	PVpredict mPredict;
	PVobserve mObserve;
	
	// my direct kalman filter	
	UD_scheme     mDirectFilter;
	
};

class NoiseFilter   // Will predict a measurement according to contant noise
{
public:
	NoiseFilter();
	~NoiseFilter();
	
	void init();
	void reinit();
	
	Real Predict();	
	void Measure(Real data);
	void setDT(Real newdt);
	
	// Variables used to tune the filter
	// prediction
	float V_NOISE;  // noise
	float V_GAMMA;  // correlation of data over dT 
	float dt;	// timestep
	// Observation
	float OBS_NOISE; // observation noise
	
private:
	
	void update();
	// Prediction and Observe model for filtering spikes.
	
	// Prediction and Observe model for Velocity, To estimate position.
	BasicPredict mPredict;
	BasicObserve mObserve;
	
	// Implementation of a kalmanfilter
	Unscented_scheme mFilter;
	

};

}// end Ei namespace



#endif // EI_BASICFILTER_H
